#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "robo/imu.h"
#include <string.h>
#include <phidget21.h>

using namespace robo;
ros::Publisher imu_pub;
int imu_interval = 500;
bool run = true;
void sigtrap(int) {run = false;}

CPhidgetSpatialHandle spatial;

//callback that will run if the Spatial generates an error
int CCONV ErrorHandler(CPhidgetHandle spatial, void *userptr, int ErrorCode, const char *unknown)
{
	printf("Error handled. %d - %s \n", ErrorCode, unknown);
	return 0;
}

int CCONV SpatialDataHandler(CPhidgetSpatialHandle spatial, void *userptr, CPhidgetSpatial_SpatialEventDataHandle *data, int count)
{
	for(int i = 0; i < count; i++)
	{
		imu raw;
		raw.acc_x = data[i]->acceleration[0];
		raw.acc_y = data[i]->acceleration[1];
		raw.acc_z = data[i]->acceleration[2];
		raw.gyro_x = data[i]->angularRate[0];
		raw.gyro_y = data[i]->angularRate[1];
		raw.gyro_z = data[i]->angularRate[2];
		raw.comp_x = data[i]->magneticField[0];
		raw.comp_y = data[i]->magneticField[1];
		raw.comp_z = data[i]->magneticField[2];
		imu_pub.publish(raw);
	}
	return 0;
}

void connect()
{
	spatial = 0;
	int result;
	const char *err;
	CPhidgetSpatial_create(&spatial);

	CPhidget_set_OnError_Handler((CPhidgetHandle)spatial, ErrorHandler, NULL);
	CPhidgetSpatial_set_OnSpatialData_Handler(spatial, SpatialDataHandler, NULL);

	//open the spatial object for device connections
	CPhidget_open((CPhidgetHandle)spatial, -1);

	//get the program to wait for a spatial device to be attached
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)spatial, 10000)))
	{
		CPhidget_getErrorDescription(result, &err);
		printf("Problem waiting for attachment: %s\n", err);
		exit(0);
	}
	
	CPhidgetSpatial_setDataRate(spatial, imu_interval);
}

void dissconnect(){
	CPhidget_close((CPhidgetHandle)spatial);
	CPhidget_delete((CPhidgetHandle)spatial);
}

void change_imu_interval(const std_msgs::Int32::ConstPtr &msg)
{
	printf("change interval to %i\n",msg->data);
	dissconnect();
	imu_interval = msg->data;
	connect();
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "imuNode");
	ros::NodeHandle n;
	imu_pub = n.advertise<imu>("/imu", 100000);
	connect();
	ros::Subscriber sub = n.subscribe("/imu_interval", 1, change_imu_interval);
	while(run){ros::spinOnce();}
	dissconnect();
	return 0;
}
